// Field & Service Robotics
// Team 5
// Mars Manipulator
// code for the microcontroller to control:
// AS12X servo @ shoudler
// AS12X servo @ elbow
// linear actuator @ wrist location
// grip servo @ gripper

// Robert Sosa & Dorian Tsai
// Nov 7 2011

//#include <SoftwareSerial.h>
#include <ax12.h>
#include <string.h>
#include <ctype.h>
#include <Servo.h>

// MESSAGE PROTOCOL:
// $As090e090v000g001;
// $As###e###v###g###;
// $ specifies the start of a new message
// A specifies actuator control
// --
// s specifies shoulder servo
// ### specifies the desired position in degrees
// e specifies elbow servo
// --
// v specifies the linear actuator
// ### specifies the desired position, from 0 (fully retracted) to 255 (fully extended)
// ex. v127; would tell the actuator to go to the halfway point
// --
// g specifies the gripper servo
// ### specifies the desired position in degrees
// ; specifies the end of the message
// ex. g090; would tell the servo to go to 90 degrees

#define MSG_END ';' // 59 = semi-colon
#define MSG_LENGTH 19 // maximum msg length
#define MSG_TYPE_ACT 'A'
#define MSG_TYPE_UNKNOWN 'U'

int ledPin = 13;
// int byteGPS=-1;

char msg[MSG_LENGTH]; // incoming serial 
char msgStartActuator[3] = "$A";
//int cont=0;
//int arm_count=0;
int conta=0;
//int indices[13];
int gen_counter = 0;
boolean msgEnd = false; // boolean to control while loop, signifying end of received serial msg
int msgPtr = 0; // index of incoming character array from serial comm

//SoftwareSerial mySerial = SoftwareSerial(2, 3);
AX12 motor_s;
AX12 motor_e;
Servo gripServo; // define gripper servo object
//int pGrip = 0; // position of gripper servo
int gripPin = 9; // grip Servo pin, PWM
int vertPin = 3; // linear actuator pin, PWN
//int pVert = 0; // position of linear actuator


void setup() {
  motor_s = AX12();
  motor_e = AX12();
  //Serial.begin (115200);  // inicializa el Serial a 115,2 Kb/s
  Serial.begin (9600);  // inicializa el Serial a 9,6 Kb/s
  
  establishContact(); // send a byte to establish contact until receiver responds                                      
  
  // initialize actuators
  AX12::init (1000000);   // inicializa los AX12 a 1 Mb/s
  gripServo.attach(gripPin);  
  gripServo.write(45); // set gripServo to default position?
  pinMode(vertPin,OUTPUT); // set linear actuator pin to output mode to ctrl actuator
  analogWrite(vertPin,0); // fully retract linear actuator 0 = retract to 255 = extend
  
   Serial.println("Initilize buffer");
   for (int i=0; i<MSG_LENGTH; i++){       // Initialize a buffer for received data
     msg[i]=' ';
   }
  Serial.println("Done");

  byte detect[2];          
  byte num = AX12::autoDetect (detect, 2); // detección de IDs

  Serial.print (" detection: ");
  Serial.println (num, DEC);

  motor_s.id = detect[0]; // asigna las ID detectadas a los motores definidos previamente
  motor_e.id = detect[1];
  motor_s.SRL = RETURN_ALL;
  motor_e.SRL = RETURN_ALL;
  
  Serial.print (" ID detectado: ");
  Serial.println (detect[0], DEC);
  Serial.print (" ID detectado: ");
  Serial.println (detect[1], DEC);
  Serial.print (" delay: ");
  AX12info test1 = motor_s.readInfo (RETURN_DELAY_TIME);
  AX12info test2 = motor_e.readInfo (RETURN_DELAY_TIME);
  Serial.println (test1.value, DEC);
  Serial.print (" error lectura: ");
  Serial.println (test1.error, DEC);
  
  int pos_s = motor_s.getPos();
  int pos_e = motor_e.getPos();
  Serial.print("Actual Shoulder angle is: ");
  Serial.println(pos_s, DEC);
  Serial.print("Actual Elbow angle is: ");
  Serial.println(pos_e, DEC);
}


void loop() {

  //int myindex =0;
  //int value[3];
  digitalWrite(ledPin, HIGH);
  
  // receive input from computer
  // until we reach the terminating character (MSG_END) & the msg length reaches 
  // the max msg length, loop
  // whenever serial data comes in, receive it and
  // put the entire received msg into the msg character array
  
  // idea: timer to go for 0.5 sec, then clear/flush the buffer
  // we don't expect the same msg after a half second
  while (!msgEnd && msgPtr < MSG_LENGTH) 
  {
    //Serial.print("Reading msg");  
    //Serial.print(msgPtr);
    //Serial.println();
    if (Serial.available() > 0) {
      msg[msgPtr] = Serial.read(); // get incoming byte (a single character)
      if (msg[msgPtr] == MSG_END) {
        msgEnd = true;      
      } else {
        msgEnd = false;      
        msgPtr++;
      }
    } 
    
    // NOTE: NEEDS ERROR HANDLING
  }
  // if we have reached the end of msg successfully
  if (msgEnd == true) { 
    // check to see correct msg has been received (sends the entire msg back to serial)
    Serial.println("Message received: ");
    Serial.print(msg);
    Serial.println();

     // now, decompose the msg into type and position
    // for the moment, there is only one type (wrist type is commented out)
    //Serial.println("Parsing msg");
    
    // allow for other types of msg types in future
    char type;
    if (msg[0] == msgStartActuator[0] && msg[1] == msgStartActuator[1]) {
      type = MSG_TYPE_ACT;
    } 
    else {
      type = MSG_TYPE_UNKNOWN;
      Serial.println("ERROR: type unknown. It was not MSG_TYPE_ACT!");
    }
          
    if (type == MSG_TYPE_ACT) { // Reads data from the array
      //Serial.println("Is Actuator command");         
      // $As###e###v###g###;
      // 0123456789
      //shoulder_angle(3);
      //elbow_angle(7);
      //vertical_position(11);
      //gripper_angle(15);
   
      for (int i=2; i<msgPtr+1; i++){
        if (msg[i]=='s'){    // check for the position of the  "," separator or the new update is looking for the command and not commas
           gen_counter = i;
           shoulder_angle();
           i = i + 3; // command number is 3 characters
        }
        else if (msg[i]== 'e'){
           gen_counter = i; 
           elbow_angle();
           i = i + 3; 
        }
        else if (msg[i]== 'v'){
           gen_counter = i;
           vertical_position();
           i = i + 3; 
        }
        else if (msg[i]== 'g'){
           gen_counter = i;
           gripper_angle(); 
           i = i + 3;
        } 
        else if (msg[i] == MSG_END){
           Serial.println("End of msg, wait for next cmd");
           //delay(500); // wait for actuator commands to follow through
        } 
        else {
          Serial.println("ERROR: No recognizable command");
        }
      } // end for loop for different acuators  
    } // end actuator type msg
    
    // else if (other types of msgs)  do other stuff
    
  }// end of msg true 
  else if (msg[0] == ' ' && msg[1] == ' ' && msg[2] == ' ' && msg[3] == ' ' && msg[4] == ' ') {
    // basically, if the msg is empty, do nothing - we are waiting for the next msg
    //Serial.println("Msg empty - do nothing.");
  }
  else {
    Serial.print("ERROR: Unkown msg - ");
    Serial.print("Message: ");
    Serial.println(msg);
  }
  
  // reset variables for loop
  //Serial.println("reset msg variables");
  msgEnd = false;
  msgPtr = 0;
  for (int i = 0; i < MSG_LENGTH; i++) 
  {
    msg[i] = ' ';
  } 
}


void shoulder_angle(){
  //int myindex2;
  //Serial.println("I am shoulder ");
           
           //for (int j=indices[0];j<(indices[1]-1);j++){
           //myindex2 = indices[0] +2;
           //}       
  int angle_s = atoi(&msg[gen_counter +1]);
  angle_s = angle_s - 90;
  
  if (angle_s > 90 || angle_s < -90) {
    Serial.print("ERROR: angle_s outside shoulder range! Abort shoulder rotate!");  
  } else {
    // just to check if the correct command was received/converted
    Serial.print("Shoulder: ");
    Serial.print(angle_s);
    Serial.println();
    
    motor_s.setVel(200);
    
    int mapped_angle = map(angle_s,-150,150,0,1023);
    //Serial.print("Writing to shoulder mapped angle: ");
    //Serial.print( mapped_angle );
    //Serial.println();
    motor_s.setPos(mapped_angle);
    
    //delay(100);
    int pos = motor_s.getPos(); 
    /*if (pos == angle_s){
      return;
    }
    else {
      motor_s.setPos (angle_s);
    }*/
  }
    //int vel = motor.getSpeed(); 
    //int load = motor.getLoad();
    //delay(100);
    //Serial.println (" ");
    //Serial.print (" Shoulder: ");
    //Serial.println (pos, DEC);
    //Serial.print (" velocidad: ");
    //Serial.println (vel, DEC);
    //Serial.print (" carga: ");
    //Serial.println (load, DEC);
}

void elbow_angle(){
  int angle_e = atoi(&msg[gen_counter +1]);
  angle_e = angle_e - 90; // subtract 90 because 90 was added to angles

  
  if (angle_e > 90 || angle_e < -90) {
    Serial.print("ERROR: angle_e outside elbow range! Abort elbow rotate!");  
  } else {
    // just to check if the correct command was received/converted
    Serial.print("Elbow: ");
    Serial.print(angle_e);
    Serial.println();
    motor_e.setVel(200);
    
    angle_e = -angle_e; // invert because of servo positioning, placed in code later so that we still see positive values
    int mapped_angle = map(angle_e,-150,150,0,1023);
    //Serial.print("Writing to elbow mapped angle: ");
    //Serial.print( mapped_angle );
    //Serial.println();
    motor_e.setPos(mapped_angle);
    //delay(100);
    int pos_2 = motor_e.getPos(); 
    /*if (pos_2 == angle_e){
       return;
    }
    else {
      motor_e.setPos (angle_e);
    } */ 
  }  
}

void vertical_position() {

  int pVert = atoi(&msg[gen_counter +1]);
  if (pVert < 0 || pVert > 255) {
    Serial.print("ERROR: pVert outside linear actuator range! Abort vertical!");  
  } else {
    // just to check if the correct command was received/converted
    Serial.print("Linear actuator: ");
    Serial.print(pVert);
    Serial.println();
    
    // write position to servo
    analogWrite(vertPin,pVert);
  }
}

void gripper_angle(){

  int pGrip = atoi(&msg[gen_counter +1]);
  if (pGrip < 80 || pGrip >= 140) {
    Serial.print("ERROR: pGrip outside servo range (80,140)!");  
  } else {
    // just to check if the correct command was received/converted
    Serial.print("Grip servo: ");
    Serial.print(pGrip);
    Serial.println();
    
    // write position to servo
    gripServo.write(pGrip);
  }
}

// send a byte to establish contact until receiver responds                                      
void establishContact() {
  while (Serial.available() <= 0) {
    Serial.print('A', BYTE);   // send a capital A
    delay(300);
  }
}
